package com.ros.autopilot;

import org.ros.concurrent.CancellableLoop;
import org.ros.internal.message.DefaultMessageFactory;
import org.ros.internal.message.RawMessage;
import org.ros.internal.message.definition.MessageDefinitionReflectionProvider;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;

import std_msgs.Float32;

public class AltitudeNode extends AbstractNodeMain {
	// Topic names
	private static final String TOPIC_BAROMETER = "barometer";

	@Override
	public GraphName getDefaultNodeName() {
		return GraphName.of("barometer");
	}

	@Override
	public void onStart(ConnectedNode connectedNode) {

		final Publisher<std_msgs.Float32> barPublisher = connectedNode
				.newPublisher(TOPIC_BAROMETER, std_msgs.Float32._TYPE);

		final DefaultMessageFactory defaultMessageFactory = new DefaultMessageFactory(
				new MessageDefinitionReflectionProvider());

		// Execute the loop
		connectedNode.executeCancellableLoop(new CancellableLoop() {

			@Override
			protected void loop() throws InterruptedException {
				Float32 msg = defaultMessageFactory
						.newFromType(std_msgs.Float32._TYPE);
				msg.setData((float) Math.random());
				barPublisher.publish(msg);
				
				
				Thread.sleep(1000);
			}
		});

	}

}
